#include "power_board_pkg/tcptalk.h"

#include <rclcpp/rclcpp.hpp> 

Tcp_Talk* Tcp_Talk::TcpInstance = NULL;
rclcpp::Node::SharedPtr Tcp_Talk::node= NULL;

Tcp_Talk::Tcp_Talk()
{
	CSocketfd = -1;
	SSocketfd = -1;
	Clientfd = -1;
	Tcp_Param_Get();

}

Tcp_Talk::~Tcp_Talk(void)
{
	Tcp_Client_Close();
	Tcp_Server_Close();
	if(TcpInstance != NULL)
		delete TcpInstance;
}

/*tcp server init*/
void Tcp_Talk::Tcp_Server_Init(void)
{
	struct sockaddr_in serveraddr;
	struct sockaddr_in clientaddr;
	int ret,on = 1;
	int flags;
	SSocketfd = socket(AF_INET,SOCK_STREAM,0);
	if (SSocketfd == -1)
	{
			std::cout << "SSocketfd create error!\n"<< std::endl;
			exit(-1);
	}
	ret = setsockopt(SSocketfd,SOL_SOCKET,SO_REUSEADDR,&on,sizeof(on));

	memset(&serveraddr,0,sizeof(serveraddr));
	serveraddr.sin_family = AF_INET;
	serveraddr.sin_port = htons(PORTNUM);
	serveraddr.sin_addr.s_addr = htonl(INADDR_ANY);

	int res = bind(SSocketfd,(struct sockaddr*)&serveraddr,sizeof(serveraddr));
	if (res == -1)
	{
			std::cout << "SSocketfd bind error!\n" << std::endl;
			exit(-1);
	}
	listen(SSocketfd,100);  
	printf("listen socket\n");
	socklen_t len = sizeof(clientaddr);
	Clientfd = accept(SSocketfd,(struct sockaddr*)&clientaddr,&len);
	if (Clientfd == -1)
	{
		std::cout << "Clientfd accept error!\n" << std::endl;
		exit(-1);
	}
	flags = fcntl(Clientfd,F_GETFL,0);
	fcntl(Clientfd,F_SETFL,flags | O_NONBLOCK);  //tcp read nonblock
}

/*tcp client init*/
/*yyy change v3.0.4*/
int Tcp_Talk::Tcp_Client_Init(void)
{
	printf("------------Tcp_Client_Init-----\n");
	double connect_time_out = Get_Curtime();  
	struct sockaddr_in serveraddr;
	int enable = 1;
	CSocketfd = socket(AF_INET, SOCK_STREAM,0);
	if(CSocketfd == -1)
	{
		std::cout<<"CSocketfd create error!\n"<<std::endl;
		exit(-1);
	}
	int ret = setsockopt(CSocketfd,IPPROTO_TCP,TCP_NODELAY,&enable,sizeof(int));  //no nagle arithmetic
	if(ret < 0)
	{
		printf("setsock nodelay error!\n");
		exit(-1);
	}
	serveraddr.sin_family = AF_INET;
	serveraddr.sin_port = htons(PortNum);
	serveraddr.sin_addr.s_addr = inet_addr(TargetIpAddr.c_str());
	printf("TargetIpAddr = %s PortNum = %d\n",TargetIpAddr.c_str(),PortNum);
	printf("tcp client wait to connect!\n");

	int flag = fcntl(CSocketfd,F_GETFL,0);  //获取当前socket的当前状态
	fcntl(CSocketfd,F_SETFL,flag|O_NONBLOCK);  //将当前的socket设置为非阻塞
	int res;
	while(!is_Timeout(&connect_time_out,100))
	{
		res = connect(CSocketfd,(struct sockaddr*)&serveraddr,sizeof(serveraddr));
		if(res < 0)
		{
			continue;
		}
		else
		{
			break;
		}
	}

	if(res < 0)
	{
		close(CSocketfd);
		printf("tcp client connect error!\n");
	}
	else
	{
		printf("tcp client connect succeed!\n");
	}

	return res;
}


/*tcp client close*/
void Tcp_Talk::Tcp_Client_Close(void)
{
	close(CSocketfd);
}

/*tcp client close*/
void Tcp_Talk::Tcp_Server_Close(void)
{
	close(SSocketfd);
	close(Clientfd);
}

/*tcp client send data*/
int Tcp_Talk::Tcp_Client_Write(unsigned char* buf,int len)
{
	//int ret = write(CSocketfd,buf,len);
	tcp_client_write_mutex.lock();
	mySleep_ms(0.5); //yyy 2020-9-17 v3.0.4 修改了延时时间 从0.1改为0.5
	int ret = send(CSocketfd,buf,len,MSG_DONTWAIT);

	if(ret < 0)
	{
		std::cerr << "write error str = " << strerror(errno) << "error = "  << errno << std::endl;
		if(errno == 11)
		{
			std::cerr << "can board tcp write error"  << std::endl; 
		}
		if(errno == 32)  //server disconnect and reinit
		{
			Tcp_Client_Close();
			Tcp_Client_Init();  //tcp client reinit
			std::cout << "tcp client reinit!" << std::endl;
		}
	}
	tcp_client_write_mutex.unlock();
	return ret;
}
/*tcp client read*/
int Tcp_Talk::Tcp_Client_Read(uint8_t* buf)
{
	int enable = 1;
	int ret = recv(CSocketfd,buf,MAXSIZE,MSG_DONTWAIT);

	setsockopt(CSocketfd,IPPROTO_TCP,TCP_QUICKACK,&enable,sizeof(enable));  //set quick ack
	// if(ret > 0)
	// {
	// 	for(int i = 0;i < ret;i ++)
	// 	{
	// 		printf("%02x ",buf[i]);
	// 	}
	// 	printf("\n");
	// }
	return ret;
}

/*tcp server read data*/
int Tcp_Talk::Tcp_Server_Read(uint8_t* buf)
{
	int enable = 1;
	int ret = recv(Clientfd,buf,MAXSIZE,MSG_DONTWAIT);
	setsockopt(Clientfd,IPPROTO_TCP,TCP_QUICKACK,&enable,sizeof(enable));  //set quick ack
	return ret;
}

/*tcp server send data*/
int Tcp_Talk::Tcp_Server_Write(unsigned char* buf,int len)
{
	int ret = write(Clientfd,buf,len);
	return ret;
}

void Tcp_Talk::Tcp_Param_Get(void)
{

	RCLCPP_INFO(node->get_logger(),"Tcp_Param_Get");
	node->declare_parameter("/param/power_board/service_ip","168.192.1.31");
	TargetIpAddr = node->get_parameter("/param/power_board/service_ip").as_string();
	node->declare_parameter("/param/power_board/service_port",50000);
	PortNum = node->get_parameter("/param/power_board/service_port").as_int();
}


